#include <math.h>
#include "./Interpolation.hpp"
float Interpolation_Class::origin_velocity = 0.0f; /* 起点速度(mm/s)(°/s) */

float Interpolation_Class::acc_distance = 0.0f;   /* 加速度段距离(mm)(°) */
float Interpolation_Class::const_distance = 0.0f; /* 匀速度段距离(mm)(°) */
float Interpolation_Class::dec_distance = 0.0f;   /* 减速度段距离(mm)(°) */
float Interpolation_Class::slow_distance = 0.0f;  /* 低速段距离(mm)(°) */

float Interpolation_Class::acc_time = 0.0f;   /* 加速段时间(s) */
float Interpolation_Class::const_time = 0.0f; /* 匀速段时间(s) */
float Interpolation_Class::dec_time = 0.0f;   /* 减速段时间(s) */
float Interpolation_Class::slow_time = 0.0f;  /* 低速段时间(s) */

/**
 * @description: 根据路径总长度和插补参数规划速度曲线
 * @param 
 *          float sum_distance      待插补距离
 * @return: 移动距离小于阈值，返回false，否则返回true
 */
bool Interpolation_Class::Init(float sum_distance)
{
    /* 保证距离为非负数 */
    if (sum_distance < 0.0f)
    {
        sum_distance = -sum_distance;
    }

    /* 判断移动距离 */
    if (sum_distance < interpolation_parameter.threshold)
    {
        /* 移动距离小于阈值 */
        interpolation_state = IS_Interpolated;
        return false;
    }

    interpolation_state = IS_Interpolating; /* 正在插补 */

    origin_velocity = interpolation_parameter.min_velocity;           /* 起始速度默认为最低速度 */
    slow_time = interpolation_parameter.slow_time;                    /* 保存低速时间 */
    slow_distance = slow_time * interpolation_parameter.min_velocity; /* 计算低速段距离 */
    if (slow_distance > sum_distance)
    {
        /* 低速段大于整段距离，全部都为低速段 */
        slow_distance = sum_distance;
        slow_time = slow_distance / interpolation_parameter.min_velocity;
        /* 计算加速度，匀速，减速度时间 */
        acc_time = 0.0f;
        const_time = 0.0f;
        dec_time = 0.0f;
        /* 计算运动距离 */
        acc_distance = 0.0f;
        const_distance = 0.0f;
        dec_distance = 0.0f;

        return true;
    }

    acc_time = (interpolation_parameter.max_velocity - origin_velocity) / interpolation_parameter.acc;                      /* 计算加速时间 */
    dec_time = (interpolation_parameter.max_velocity - interpolation_parameter.min_velocity) / interpolation_parameter.dec; /* 计算减速时间 */

    acc_distance = (interpolation_parameter.max_velocity + origin_velocity) * acc_time / 2.0f;                      /* 计算加速段距离 */
    dec_distance = (interpolation_parameter.max_velocity + interpolation_parameter.min_velocity) * dec_time / 2.0f; /* 计算减速段距离 */

    /* 以下计算已经去除了低速阶段 */
    float sum_distance_no_slow = sum_distance - slow_distance; /* 没有低速段的移动距离 */
    if ((acc_distance + dec_distance) <= sum_distance_no_slow)
    {
        /* 情况一，加速，匀速，减速个阶段均有
        |
        |  ______
        | /      \
        |/        \
        |          \
        +—————————————————————
        */

        const_distance = sum_distance_no_slow - (acc_distance + dec_distance);
        const_time = const_distance / interpolation_parameter.max_velocity;
    }
    else
    {
        /* 情况二，没有匀速阶段
            |
            |  /\
            | /  \
            |/    \
            |      \
            +—————————————————
        */
        const_time = 0.0f;
        const_distance = 0.0f;

        /* 更新最大速度 */
        float v_max, temp;
        temp = 2 * sum_distance_no_slow + (origin_velocity * origin_velocity / interpolation_parameter.acc) +
               (interpolation_parameter.min_velocity * interpolation_parameter.min_velocity / interpolation_parameter.dec);
        v_max = temp / (1.0f / interpolation_parameter.acc + 1.0f / interpolation_parameter.dec);
        interpolation_parameter.max_velocity = sqrtf(v_max);

        acc_time = (interpolation_parameter.max_velocity - origin_velocity) / interpolation_parameter.acc;                      /* 计算加速时间 */
        dec_time = (interpolation_parameter.max_velocity - interpolation_parameter.min_velocity) / interpolation_parameter.dec; /* 计算减速时间 */

        acc_distance = (interpolation_parameter.max_velocity + origin_velocity) * acc_time / 2.0f;                      /* 计算加速段距离 */
        dec_distance = (interpolation_parameter.max_velocity + interpolation_parameter.min_velocity) * dec_time / 2.0f; /* 计算减速段距离 */
    }

    return true;
}

/**
 * @description: 根据当前位移计算插补速度
 * @param {type} 
 * @return: 
 */
float Interpolation_Class::Cal_Velocity(const float current_distance)
{
    float velocity_temp = 0.0f;
    interpolation_state = IS_Interpolating;
    if (current_distance < -5.0f)
    {
        /* 距离为负数 */
        velocity_temp = origin_velocity;
    }
    else if (current_distance < 0.0f)
    {
        velocity_temp = origin_velocity;
    }
    else if (current_distance < acc_distance)
    {
        /* 在加速区 */
        velocity_temp = 2 * interpolation_parameter.acc * current_distance + origin_velocity * origin_velocity;
        velocity_temp = sqrtf(velocity_temp);
    }
    else if (current_distance < (acc_distance + const_distance))
    {
        /* 在匀速区 */
        velocity_temp = interpolation_parameter.max_velocity;
    }
    else if (current_distance < (acc_distance + const_distance + dec_distance))
    {
        /* 在减速区 */
        velocity_temp = interpolation_parameter.max_velocity * interpolation_parameter.max_velocity - 2 * (current_distance - acc_distance - const_distance) * interpolation_parameter.dec;
        velocity_temp = sqrtf(velocity_temp);
    }
    else if (current_distance < (acc_distance + const_distance + dec_distance + slow_distance))
    {
        /* 在低速区 */
        velocity_temp = interpolation_parameter.min_velocity;
    }
    else
    {
        /* 插补完成 */
        velocity_temp = interpolation_parameter.min_velocity;
        interpolation_state = IS_Interpolated;
    }
    return velocity_temp;
}
